///////////////////////////////////////AUTOAIM(multi_thread)///////////////////////////////////////////////
// 1.添加ROI,增加图像处理速度
/////////////////////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/opencv.hpp>
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "camera/MindVision.h"
#include "assert/Timer/Timer.h"
#include "autoaim/armor/Armor.h"
#include "autoaim/detector/Detector.h"
#include "serial/serial.h"
#include "autoaim/data_pack/DataPack.h"
#include "autoaim/computor/Computor.h"
#include <string>
#include <stdio.h>
#include <Eigen/Dense>

using namespace cv;
using namespace std;
using namespace Eigen;

typedef struct Inform
{
    Mat image;
    DataPack datapack;
    double time;
} Inform;

Timer clock_;
MindVision camera;
Detector detector;
string port = "/dev/ttyTHS2";
Serialport serial(port);
Computor computor;
vector<Inform> Inform_queue;

static void *Input_Inform(void *)
{
    Inform inform;
    while (true)
    {
        clock_.begin();
        camera.getImage(inform.image);
        // serial.clear();
        serial.readStatusTo(inform.datapack);
        inform.time = clock_.thisTime();

        Inform_queue.push_back(inform);
        clock_.end();
        cout<<"____________________________camera:"<<clock_.timeConsuming(MILLISECOND)<<"ms"<<endl;
    }
    pthread_exit(NULL);
    return NULL;
}

static void *Output_Inform(void *)
{
    Inform inform;
    vector<Inform>::iterator itr;
    int queue_size = 0;
    while (true)
    {
        itr = Inform_queue.begin();
        if (Inform_queue.size() == 0)
        {
            continue;
        }
        inform = Inform_queue.back();
        Inform_queue.clear();
// queue_size=Inform_queue.size();
// for(int i=0;i<Inform_queue.size();i++)
// {
//     itr=Inform_queue.erase(itr);
// }

        detector.getResult(inform.image);
#ifdef ADJUST
        waitKey(1);
#endif
         computor.updateArmor(detector);
         computor.updateStatus(inform.datapack);
         computor.getTarget();
         computor.sendData(serial);
    }
    pthread_exit(NULL);
    return NULL;
}
int main()
{
    pthread_t TD[2];
    int ret=1;
    ret=pthread_create(&TD[0],NULL,Input_Inform,NULL);
    if(ret!=0)
    {
        cout<<"error:Input_Inform create failed"<<endl;
    }
    ret=pthread_create(&TD[1],NULL,Output_Inform,NULL);
    if(ret!=0)
    {
        cout<<"error:Output_Inform create failed"<<endl;
    }
    pthread_join(TD[0],NULL);
    pthread_join(TD[1],NULL);

    return 0;
}
///////////////////////////////////////AUTOAIM(single_thread)/////////////////////////////////////////////////////////

// #include <iostream>
// #include <opencv2/imgproc/types_c.h>
// #include <opencv2/opencv.hpp>
// #include "opencv2/core/core.hpp"
// #include "opencv2/highgui/highgui.hpp"
// #include "camera/MindVision.h"
// #include "assert/Timer/Timer.h"
// #include "autoaim/armor/Armor.h"
// #include "autoaim/detector/Detector.h"
// #include "serial/serial.h"
// #include "autoaim/data_pack/DataPack.h"
// #include "autoaim/computor/Computor.h"
// #include <string>
// #include <stdio.h>
// #include <Eigen/Dense>

// using namespace cv;
// using namespace std;
// using namespace Eigen;

// typedef struct Inform
// {
//     Mat image;
//     DataPack datapack;
//     double time;
// } Inform;

// DataPack datapack0;
// Timer clock_;
// MindVision camera;
// Detector detector;
// string port = "/dev/ttyTHS2";
// Serialport serial(port);
// Computor computor;
// vector<Inform> Inform_queue;

// static void *Input_Inform(void *)
// {
//     Inform inform;
//     while (true)
//     {
//         clock_.begin();
//         camera.getImage(inform.image);
//         serial.clear();
//         serial.readStatusTo(inform.datapack);
//         inform.time = clock_.thisTime();

//         Inform_queue.push_back(inform);
//         clock_.end();
//         cout<<"____________________________camera:"<<clock_.timeConsuming(MILLISECOND)<<"ms"<<endl;
//     }
//     pthread_exit(NULL);
//     return NULL;
// }

// static void *Output_Inform(void *)
// {
//     Inform inform;
//     vector<Inform>::iterator itr;
//     int queue_size = 0;
//     while (true)
//     {
//         itr = Inform_queue.begin();
//         if (Inform_queue.size() == 0)
//         {
//             continue;
//         }
//         inform = Inform_queue.back();
//         Inform_queue.clear();
// queue_size=Inform_queue.size();
// for(int i=0;i<Inform_queue.size();i++)
// {
//     itr=Inform_queue.erase(itr);
// }

//         detector.getResult(inform.image);
// #ifdef ADJUST
//         waitKey(1);
// #endif
//          computor.updateArmor(detector);
//          computor.updateStatus(inform.datapack);
//          computor.getTarget();
//          computor.sendData(serial);
//     }
//     pthread_exit(NULL);
//     return NULL;
// }
// int main()
// {
//     pthread_t TD[2];
//     int ret=1;
//     ret=pthread_create(&TD[0],NULL,Input_Inform,NULL);
//     if(ret!=0)
//     {
//         cout<<"error:Input_Inform create failed"<<endl;
//     }
//     ret=pthread_create(&TD[1],NULL,Output_Inform,NULL);
//     if(ret!=0)
//     {
//         cout<<"error:Output_Inform create failed"<<endl;
//     }
//     pthread_join(TD[0],NULL);
//     pthread_join(TD[1],NULL);
//     float angle[2]={0.005,0.006};
//     while(true)
//     {
//         serial.sendAngle(angle);
//         serial.clear();
//         serial.readStatusTo(datapack0);
//         if(datapack0.isRead==true)
//         {
//             cout<<"pitch="<<datapack0.euler_angle[1]<<endl;
//             datapack0.isRead=false;
//         }
//         else{
//             cout<<"***"<<endl;
//         }
//         waitKey(15);
//     }
//     return 0;
// }
///////////////////////////////////////EXPERIMENT///////////////////////////////////////
// #include <iostream>
// #include <opencv2/imgproc/types_c.h>
// #include <opencv2/opencv.hpp>
// #include "opencv2/core/core.hpp"
// #include "opencv2/highgui/highgui.hpp"
// #include "camera/MindVision.h"
// #include "assert/Timer/Timer.h"
// #include "autoaim/armor/Armor.h"
// #include "autoaim/detector/Detector.h"
// #include "serial/serial.h"
// #include "autoaim/data_pack/DataPack.h"
// #include "autoaim/computor/Computor.h"
// #include <string>
// #include <stdio.h>
// #include <fstream>

// using namespace cv;
// using namespace std;

// int main()
// {
//     int count=0;
//     ofstream angle_pitch0;
//     float pitch0 = 0.25;
//     namedWindow("");
//     angle_pitch0.open("/home/dji/Project/AutoAim(shaobingxia)/assert/trajectory/angle_pitch.txt", ios::app | ios::out);
//     if(angle_pitch0.is_open()==false)
//     {
//         cout<<"read failed"<<endl;
//     }
//     while (true)
//     {
//         char input=waitKey(1);
//         cout<<"input="<<(int)input<<endl;

//         if (input == 'r')
//         {
//             count++;

//             angle_pitch0 << pitch0 << endl;
//         }
//         cout<<"the "<<count<<" th"<<endl;
//         if (input == 'q')
//         {
//             angle_pitch0.close();
//             cout<<"save and exit"<<endl;
//             break;
//         }
//     }
//     return 0;
// }

//////////////////////////////////////CALIBRATE IMAGE////////////////////////////////////////////
// #include <iostream>
// #include <opencv2/imgproc/types_c.h>
// #include <opencv2/opencv.hpp>
// #include "opencv2/core/core.hpp"
// #include "opencv2/highgui/highgui.hpp"
// #include "camera/MindVision.h"
// #include "assert/Timer/Timer.h"
// #include "autoaim/armor/Armor.h"
// #include "autoaim/detector/Detector.h"
// #include "serial/serial.h"
// #include "autoaim/data_pack/DataPack.h"
// #include "autoaim/computor/Computor.h"
// #include <string>
// #include <stdio.h>
// #include <fstream>

// using namespace cv;
// using namespace std;
// int main()
// {
//     int num = 1;
//     // char total[100];
//     Mat image;
//     MindVision camera;
//     // camera.open();

//     while (true)
//     {
//         camera.getImage(image);
//         imshow("save image", image);
//         int input = waitKey(50);
//         //snprintf(total, 100, "/home/dji/Project/AutoAim(shaobingxia)/image/calib_image%d.jpg", num);
//         if (input == ' ')
//         {
//             cout << endl;
//             cout << endl;
//             cout << endl;
//             cout << endl;
//             cout<<"00000000000000000000000000000000000000000000"<<endl;
//             num++;
//         }
//         else if (input == 'q')
//         {
//             break;
//         }
//         cout << endl;
//         cout << "preparing calib_image" << num << "." << endl;
//     }
//     cout << endl;
//     return 0;
// }

//////////////////////////////////////////SVM/////////////////////////////////////////////
// #include <opencv2/opencv.hpp>
// #include <iostream>
// #include <opencv2/core/core.hpp>
// #include <opencv2/highgui/highgui.hpp>
// #include <opencv2/ml/ml.hpp>

// ///////////////////////////////////////STL////////////////////////////////////
// #include <vector>
// #include <iostream>
// #include <algorithm>
// #include <string>
// #include <opencv2/imgproc/types_c.h>
// #include <opencv2/opencv.hpp>
// #include "opencv2/core/core.hpp"
// #include "opencv2/highgui/highgui.hpp"
// #include "camera/MindVision.h"
// #include "assert/Timer/Timer.h"
// #include <string.h>
// using namespace std;
// using namespace cv;

// int main(int argc,char* argv[])
// {
//     Timer clock;
//     clock.begin();

//     imshow("test",img);
//     waitKey(0);
//     clock.end();
//     std::cout<<"using time "<<clock.timeConsuming()<<" milliseconds" << std::endl;
//     return 0;

// }

//////////////////////////////////////////////SHOW TIMER ACCURACY///////////////////////////////////////////
// int main()
// {
//     Timer clock1;
//     while(true)
//     {
//         clock1.begin();
//         clock1.end();
//         std::cout<<"class Timer's accuracy is "<<clock1.timeConsuming(NANOSECOND)<< "nanosecond"<<std::endl;
//     }
//     return 0;
// }
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// g++ main.cpp ./assert/Timer/Timer.cpp ./camera/MindVision.cpp -I./assert/Timer -I./camera -lMVSDK -o test `pkg-config --cflags --libs opencv4`
